
NVIC_INT_CTRL    EQU  		 	0xE000ED04                 
NVIC_SYSPRI14    EQU  			0xE000ED22                 
NVIC_PENDSV_PRI  EQU  			      0xFF                     
NVIC_PENDSVSET   EQU  		    0x10000000 
		
	AREA |.text|, CODE, READONLY, ALIGN=2
		
	THUMB
	REQUIRE8 
    PRESERVE8
		
get_r9\
	PROC
	EXPORT get_r9
		
	MOV 	R0, R9
	BX		LR
	ENDP
		
set_r9\
	PROC
	EXPORT set_r9
		
	MOV		R9, R0
	BX		LR
	ENDP
		
OS_CPU_SR_Save\
	PROC
	EXPORT	OS_CPU_SR_Save
	
    MRS     R0, PRIMASK                                         ; Set prio int mask to mask all (except faults)
    CPSID   I
    BX      LR
	ENDP

OS_CPU_SR_Restore\
	PROC
	EXPORT 	OS_CPU_SR_Restore
		
    MSR     PRIMASK, R0
    BX      LR
	ENDP

OSCtxSw\
	PROC
	EXPORT	OSCtxSw
		
    LDR     R0, =NVIC_INT_CTRL                                 
    LDR     R1, =NVIC_PENDSVSET
    STR     R1, [R0]
    BX      LR
	ENDP

OSIntCtxSw\
	PROC
	EXPORT	OSIntCtxSw
		
    LDR     R0, =NVIC_INT_CTRL        ; Trigger the PendSV exception (causes context switch)
    LDR     R1, =NVIC_PENDSVSET
    STR     R1, [R0]
    BX      LR
	ENDP

OSStartHighRdy\
	PROC
	EXPORT	OSStartHighRdy
		
    LDR     R0, =NVIC_SYSPRI14                               
    LDR     R1, =NVIC_PENDSV_PRI
    STRB    R1, [R0]

    MOVS    R0, #0                                      
    MSR     PSP, R0

    LDR     R0, =NVIC_INT_CTRL       
    LDR     R1, =NVIC_PENDSVSET
    STR     R1, [R0]

    CPSIE   I                

OSStartHang
    B       OSStartHang 
	ENDP

OS_CPU_PendSVHandler\
	PROC
	EXPORT	OS_CPU_PendSVHandler
    EXTERN 	OSTCBCur
    EXTERN 	OSTCBHighRdy

    CPSID 	I
    MRS   	R0, PSP
    CBZ   	R0, OS_CPU_PendSVHandler_nosave

    SUBS  	R0, R0, #0x20
    STM   	R0,{R4-R11}

    LDR   	R1, =OSTCBCur
    LDR   	R1, [R1]
    STR   	R0, [R1]

OS_CPU_PendSVHandler_nosave
    LDR     R0, =OSTCBCur                   ; OSTCBCur  = OSTCBHighRdy;
    LDR     R1, =OSTCBHighRdy
    LDR     R2, [R1]
    STR     R2, [R0]

    LDR     R0, [R2]                        ; R0 is new process SP; SP = OSTCBHighRdy->OSTCBStkPtr;
    LDM     R0, {R4-R11}                    ; Restore r4-11 from new process stack
    ADDS    R0, R0, #0x20
    MSR     PSP, R0                         ; Load PSP with new process SP
    ORR     LR, LR, #0x04                   ; Ensure exception return uses process stack
    CPSIE   I
    BX      LR                              ; Exception return will restore remaining context

	ENDP
		